#include "coos.inc"

SWITCH_TASK code;pcl_call,pch_call
;save cur pcl/pch,check dpc,check next task,run idle
    clrwdt;clear watch dog timer
    clrf status;clear status flag;

    btfsc soft_status,0;check bit dpc
    goto _DPC

    movf last_ready_tcb,w;if no active task, switch to idle
    btfsc status,z
    goto _IDLE

    movf current_tcb,w;save current pch/pcl/t_status
    btfss status,z;wrap ptr if no current tcb
    goto _WRAP_PTR
    movwf fsr
    movf pch_call,w
    movwf indf
    movf pcl_call,w
    incf fsr
    movwf indf

    incf fsr;check next tcb
    incf fsr
    subwf fsr,w;w=fsr-w
    btfsc status,c;if fsr>w _wrap_ptr;
    goto _SWITCH

    _WRAP_PTR:
    movlw tcb_arrays
    movwf fsr
    _SWITCH:
    movf fsr,w
    movwf current_tcb
    movf indf,w
    movwf pclath;pch
    incf fsr
    movf indf,w
    movwf pcl;task switched
    _IDLE:
    pagesel IDLE
    goto IDLE
    _DPC:
    pagesel DPC
    goto DPC

WAIT_SEMAPHORE code;param1: signal id
;check signal, if not set then move tcb into wait array, else decrease signal and continue
    clrf status;switch to bank0
    movf param1,w;check if simaphore arised
    addwf signal_array,w;dereference semaphore
    movwf fsr
    movf indf,f;test if indf!=0 _ARAISE else await
    btfss status,z
    goto _AWAIT
    decf indf;dec semaphore
    goto _WAIT_RET

    _AWAIT:
    movlw SZ_TCB;copy cur tcb from ready array into wait array
    movwf c_temp
    subwf last_wait_tcb,w;last_wait_tcb-=SZ_TCB
    movf d_temp;d_temp=last_wait_tcb
    movf last_wait_tcb;
    movwf current_tcb,w;
    movf s_temp;s_temp=current_tcb
    MEMCPY

    movwf current_tcb,w
    subwf last_ready_tcb,w
    btfss status,z
    goto _WAIT_POST
    movwf current_tcb,w;remove from ready array,increase last_ready_tcb
    movf d_temp
    movwf last_ready_tcb,w
    movf s_temp
    movlw SZ_TCB
    movf c_temp
    MEMCPY
    _WAIT_POST:
    movlw SZ_TCB
    subwf last_wait_tcb,f;last_wait_tcb-=SZ_TCB
    subwf last_ready_tcb,f;last_ready_tcb-=SZ_TCB
    clrf current_tcb;current_tcb=0

    _WAIT_RET:
    pagesel SWITCH_TASK
    goto SWITCH_TASK

SET_EVENT code;param1: signal_id param2: b_pulse
;check signal, if not set then move tcb into ready array, increase signal
    clrf status;switch to bank0
    movf param1,w;check if simaphore arised
    movwf c_temp
    addwf signal_array,w;dereference semaphore
    movwf fsr
    incf indf
    btfss status,z;if ++indf==0 indf=0xff
    decf indf

    movlw 1;NO2BIT
    movf b_temp
    movwf c_temp,f
    btfsc status,z
    goto _SHIFT_EXIT
    _SHIFT_LEFT:
    rlf b_temp,f
    decf c_temp,f
    btfss status,z
    goto _SHIFT_LEFT
    _SHIF_EXIT:

    movwf last_wait_tcb,w;test every wait tcb if awaiting specified semaphore
    addlw PZ_STATUS
    movf s_temp
    _WAIT_LOOP:
    sublw tcb_array+SZ_TCBARRARY;test if reached to tail of tcb_wait_array
    btfsc status,c
    goto _EVENT_EXIT
    movwf s_temp,w
    movf fsr
    movwf indf,w
    andwf b_temp,w
    btfsc status,z
    goto _WAIT_ACTIVATE;
    movwf s_temp,w
    addlw SZ_TCB,w
    goto _WAIT_LOOP
    _WAIT_ACTIVATE:;copy to tail of ready array, remove from wait array
    movlw SZ_TCB
    movwf c_temp,w
    movwf s_temp,w
    sublw PZ_STATUS
    _WAIT_MOV_RDY:
    decf c_temp,f
    btfsc status,z
    goto _WAIT_LOOP
    movf s_temp,f
    movf fsr,f
    movwf indf,w
    movf w_temp
    movwf last_ready_tcb
    goto _WAIT_MOV_RDY
    _EVENT_EXIT:
    ;process pulse
    pagesel SWITCH_TASK
    goto SWITCH_TASK

RESET_EVENT code;param 1 signal id
;
    clrf status

REST_VECT CODE 0x0000;
    pagesel MAIN
    goto MAIN

MAIN

    GOTO $                          ; loop forever

    END

